/*
 * File: f_5pos.c
 *
 * MATLAB Coder version            : 5.0
 * C/C++ source code generated on  : 26-Nov-2021 16:21:23
 */

/* Include Files */
#include "f_5pos.h"
#include "rt_nonfinite.h"
#include <float.h>
#include <math.h>

/* Variable Definitions */
static boolean_T isInitialized_f_5pos = false;

/* Function Declarations */
static void b_cosd(float *x);
static void b_sind(float *x);
static float rt_remf_snf(float u0, float u1);

/* Function Definitions */

/*
 * Arguments    : float *x
 * Return Type  : void
 */
static void b_cosd(float *x)
{
  float absx;
  signed char n;
  if (rtIsInfF(*x) || rtIsNaNF(*x)) {
    *x = rtNaNF;
  } else {
    *x = rt_remf_snf(*x, 360.0F);
    absx = fabsf(*x);
    if (absx > 180.0F) {
      if (*x > 0.0F) {
        *x -= 360.0F;
      } else {
        *x += 360.0F;
      }

      absx = fabsf(*x);
    }

    if (absx <= 45.0F) {
      *x *= 0.0174532924F;
      n = 0;
    } else if (absx <= 135.0F) {
      if (*x > 0.0F) {
        *x = 0.0174532924F * (*x - 90.0F);
        n = 1;
      } else {
        *x = 0.0174532924F * (*x + 90.0F);
        n = -1;
      }
    } else if (*x > 0.0F) {
      *x = 0.0174532924F * (*x - 180.0F);
      n = 2;
    } else {
      *x = 0.0174532924F * (*x + 180.0F);
      n = -2;
    }

    if (n == 0) {
      *x = cosf(*x);
    } else if (n == 1) {
      *x = -sinf(*x);
    } else if (n == -1) {
      *x = sinf(*x);
    } else {
      *x = -cosf(*x);
    }
  }
}

/*
 * Arguments    : float *x
 * Return Type  : void
 */
static void b_sind(float *x)
{
  float absx;
  signed char n;
  if (rtIsInfF(*x) || rtIsNaNF(*x)) {
    *x = rtNaNF;
  } else {
    *x = rt_remf_snf(*x, 360.0F);
    absx = fabsf(*x);
    if (absx > 180.0F) {
      if (*x > 0.0F) {
        *x -= 360.0F;
      } else {
        *x += 360.0F;
      }

      absx = fabsf(*x);
    }

    if (absx <= 45.0F) {
      *x *= 0.0174532924F;
      n = 0;
    } else if (absx <= 135.0F) {
      if (*x > 0.0F) {
        *x = 0.0174532924F * (*x - 90.0F);
        n = 1;
      } else {
        *x = 0.0174532924F * (*x + 90.0F);
        n = -1;
      }
    } else if (*x > 0.0F) {
      *x = 0.0174532924F * (*x - 180.0F);
      n = 2;
    } else {
      *x = 0.0174532924F * (*x + 180.0F);
      n = -2;
    }

    if (n == 0) {
      *x = sinf(*x);
    } else if (n == 1) {
      *x = cosf(*x);
    } else if (n == -1) {
      *x = -cosf(*x);
    } else {
      *x = -sinf(*x);
    }
  }
}

/*
 * Arguments    : float u0
 *                float u1
 * Return Type  : float
 */
static float rt_remf_snf(float u0, float u1)
{
  float y;
  float q;
  if (rtIsNaNF(u0) || rtIsNaNF(u1) || rtIsInfF(u0)) {
    y = rtNaNF;
  } else if (rtIsInfF(u1)) {
    y = u0;
  } else if ((u1 != 0.0F) && (u1 != truncf(u1))) {
    q = fabsf(u0 / u1);
    if (!(fabsf(q - floorf(q + 0.5F)) > FLT_EPSILON * q)) {
      y = 0.0F * u0;
    } else {
      y = fmodf(u0, u1);
    }
  } else {
    y = fmodf(u0, u1);
  }

  return y;
}

/*
 * Arguments    : float Alpha1
 *                float Alpha2
 *                const float Param[6]
 *                float *Xe
 *                float *Ye
 * Return Type  : void
 */
void f_5pos(float Alpha1, float Alpha2, const float Param[6], float *Xe, float
            *Ye)
{
  float a_tmp;
  float Xa;
  float Ya;
  float C;
  float thate1;
  float B;
  float A;
  if (!isInitialized_f_5pos) {
    f_5pos_initialize();
  }

  a_tmp = Alpha1;
  b_cosd(&a_tmp);
  Xa = Param[0] * a_tmp;
  a_tmp = Alpha1;
  b_sind(&a_tmp);
  Ya = Param[0] * a_tmp;
  a_tmp = Alpha2;
  b_cosd(&a_tmp);
  C = Alpha2;
  b_sind(&C);
  a_tmp = (Param[4] + Param[3] * a_tmp) - Xa;
  C = Param[3] * C - Ya;
  thate1 = sqrtf(a_tmp * a_tmp + C * C);
  B = 2.0F * Param[1];
  A = B * a_tmp;
  B *= C;
  C = (Param[1] * Param[1] + thate1 * thate1) - Param[2] * Param[2];
  if (Alpha1 == -180.0F) {
    C = (-180.0F - Alpha2) / 2.0F;
    a_tmp = C;
    b_sind(&a_tmp);
    thate1 = -fabsf(-180.0F - (C + 57.2957802F * asinf(a_tmp / 2.0F)));
  } else if (Alpha1 == 180.0F) {
    C = (Alpha2 + 180.0F) / 2.0F;
    a_tmp = C;
    b_sind(&a_tmp);
    thate1 = -fabsf(180.0F - (C + 57.2957802F * asinf(a_tmp / 2.0F)));
  } else if (Alpha2 == 0.0F) {
    if (0.0F < Alpha1) {
      a_tmp = 180.0F - Alpha1 / 2.0F;
      C = a_tmp;
      b_sind(&C);
      thate1 = Alpha1 + (a_tmp + 57.2957802F * asinf(C / 2.0F));
    } else {
      C = Alpha1 / 2.0F;
      a_tmp = C;
      b_sind(&a_tmp);
      thate1 = Alpha1 - (C + 57.2957802F * asinf(a_tmp / 2.0F));
    }
  } else {
    a_tmp = sqrtf((A * A + B * B) - C * C);
    C += A;
    thate1 = 2.0F * (57.2957802F * atanf((B + a_tmp) / C));
    if (Alpha1 - thate1 < 0.0F) {
      thate1 = 2.0F * (57.2957802F * atanf((B - a_tmp) / C));
    }
  }

  a_tmp = thate1;
  b_cosd(&a_tmp);
  C = Param[1] + Param[5];
  *Xe = Xa + C * a_tmp;
  b_sind(&thate1);
  *Ye = Ya + C * thate1;

  /* O */
  /* D */
  /* A */
  /* C */
  /* B */
  /* E */
}

/*
 * Arguments    : void
 * Return Type  : void
 */
void f_5pos_initialize(void)
{
  //rt_InitInfAndNaN();
  isInitialized_f_5pos = true;
}

/*
 * Arguments    : void
 * Return Type  : void
 */
void f_5pos_terminate(void)
{
  /* (no terminate code required) */
  isInitialized_f_5pos = false;
}

/*
 * File trailer for f_5pos.c
 *
 * [EOF]
 */
